//
//  Player.cpp
//  TrackingSim
//
//  Created by Garrett Manka on 11/24/11.
//  Copyright (c) 2011. All rights reserved.
//

#include <sim/Player.h>
#include <iostream>

namespace Sim
{
    const double pi = 3.14159265358979;
    const double to_rad = pi / 180.0;
    const double to_deg = 180.0 / pi;
    
    unsigned int Player::s_playercount = 0;
    const double Player::s_turn_rad = 5.0 * to_rad;
    const double Player::s_accel_factor = 5.0;
    
    Player::Player():m_playernum(s_playercount),
                     m_sqt(1.0,Quaternion(Vector(),1.0),Vector())
    {
        s_playercount++;
    }
    
    Player::Player(const std::string& name, Vector pos, Vector vel, Vector up):
    m_playernum(s_playercount),
    m_name(name),
    m_velocity(vel),
    m_velocity_previous(vel),
    m_acceleration(Vector()),
    m_up(up),
    m_sqt(1.0,Quaternion(Vector(),1.0),pos),
    m_view(vel),
    m_left( cross(up,vel).unit() ),
    m_axis_color(Vector(1.0,1.0,1.0) )
    {
        s_playercount++;
    }
    
    Player::Player(Vector& pos, Vector& vel, Vector& accel, Vector& up):
    m_playernum(s_playercount),
    m_velocity(vel),
    m_velocity_previous(vel),
    m_acceleration(accel),
    m_up(up),
    m_sqt(1.0,Quaternion(Vector(),1.0),pos),
    m_view(vel),
    m_left( cross(up,vel).unit() ),
    m_axis_color(Vector(1.0,1.0,1.0) )
    {
        s_playercount++;
    }
    
    Player::Player( const Player& p )
    {
        m_name = p.m_name;
        m_mesh = p.m_mesh;
        m_playernum = p.m_playernum;
        m_velocity = p.m_velocity;
        m_velocity_previous = p.m_velocity_previous;
        m_acceleration = p.m_acceleration;
        m_up = p.m_up;
        m_view = p.m_view;
        m_left = p.m_left;
        m_axis_color = p.m_axis_color;
        m_sqt = m_sqt;
        m_world_to_local = p.m_world_to_local;
    }

	Player::~Player()
	{

	}
    
    unsigned int Player::getPlayerNum() const
    {
        return m_playernum;
    }
    
    const std::string& Player::getName() const
    {
        return m_name;
    }
    
    const std::string& Player::getMesh() const
    {
        return m_mesh;
    }
    
    const Vector& Player::getPosition() const
    {
        return m_sqt.t();
    }
    
    const Vector& Player::getVelocity() const
    {
        return m_velocity;
    }
    
    const Vector& Player::getAcceleration() const
    {
        return m_acceleration;
    }
    
    const Vector& Player::getUp() const
    {
        return m_up;
    }
    
    const Vector& Player::getView() const
    {
        return m_view;
    }
    
    const Vector& Player::getLeft() const
    {
        return m_left;
    }
    
    const Vector& Player::getAxisColor() const
    {
        return m_axis_color;
    }
    
    const SQT& Player::getSQT() const 
    {
        return m_sqt;
    }
    
    Matrix4x4& Player::getWorldToLocal()
    {
        Matrix4x4 wTol( m_left.x() , m_up.x() , m_view.x() , getPosition().x() ,
                        m_left.y() , m_up.y() , m_view.y() , getPosition().y() , 
                        m_left.z() , m_up.z() , m_view.z() , getPosition().z() , 
                                 0 ,        0 ,          0 ,                 1 );
        
        m_world_to_local = wTol;
        
        return m_world_to_local;
    
    }
    
    void Player::setMesh(std::string& mesh)
    {
        m_mesh = mesh;
    }
    
    void Player::setPosition(const Sim::Vector &pos)
    {
        m_sqt.t( pos );
    }
    
    void Player::setVelocity(const Sim::Vector &vel)
    {
        m_velocity = vel;
    }
    
    void Player::setAcceleration(const Sim::Vector &accel)
    {
        m_acceleration = accel;
    }
    
    void Player::setUp(const Sim::Vector &up)
    {
        m_up = up;
    }
    
    void Player::setView(const Sim::Vector &view)
    {
        m_view = view;
    }
    
    void Player::setLeft(const Sim::Vector &left)
    {
        m_left = left;
    }
    
    void Player::setAxisColor(const Sim::Vector &color)
    {
        m_axis_color = color;
    }
    
    void Player::stop()
    {
        m_velocity_previous = m_velocity_previous;
        m_velocity = Vector(0.001, 0.001, 0.001);
        m_acceleration = Vector(0.001, 0.001, 0.001);
    }
    
    void Player::resume()
    {
        m_velocity = m_velocity_previous.mag() * m_view.unit();
    }
    
    void Player::accelerate()
    {
        if( m_velocity.mag() > 0 )
        {
            m_velocity = 1.5 * m_velocity;
        }
        else
        {
            m_velocity = m_view.unit();
        }
        
    }
    
    void Player::decelerate()
    {
        if( m_velocity.mag() > 0 )
        {
            m_velocity = 0.5 * m_velocity;
        }
        else
        {
            m_velocity = m_view.unit();
        }
    }
    
    void Player::turnLeft()
    {
        rotateYaw(s_turn_rad);
    }
    
    void Player::turnRight()
    {
        rotateYaw(-s_turn_rad);
    }
    
    void Player::rollLeft()
    {
        rotateRoll(s_turn_rad);
    }
    
    void Player::rollRight()
    {
        rotateRoll(-s_turn_rad);
    }
    
    void Player::pitchUp()
    {
        rotatePitch(s_turn_rad);
    }
    
    void Player::pitchDown()
    {
        rotatePitch(-s_turn_rad);
    }
    
    void Player::rotateYaw( double theta )
    {
        rotateVector(m_view, m_up, theta);
        
        m_left = cross( m_up , m_view ).unit();
        
        m_velocity = m_velocity.mag() * m_view.unit();
    }
    
    void Player::rotateRoll( double theta )
    {
        rotateVector(m_up, m_view, theta);
        
        m_left = cross( m_up , m_view ).unit();
        
        m_velocity = m_velocity.mag() * m_view.unit();
    }
    
    void Player::rotatePitch( double theta )
    {
        rotateVector(m_view, m_left, theta);
        
        m_up = cross( m_view , m_left ).unit();
        
        m_velocity = m_velocity.mag() * m_view.unit();
    }
    
    
    void Player::printPos()
    {
        std::cout<<"Pos: x: " << m_sqt.t().x() << " y: " << m_sqt.t().y() << " z: " << m_sqt.t().z() << std::endl;
    }
    
    void Player::printVel()
    {
        std::cout<<"Vel: x: " << m_velocity.x() << " y: " << m_velocity.y() << " z: " << m_velocity.z() << std::endl;
    }
    
    void Player::printAccel()
    {
        std::cout<<"Accel: x: " << m_acceleration.x() << " y: " << m_acceleration.y() << " z: " << m_acceleration.z() << std::endl;
    }
    
}